from functools import reduce
import math
from TestShipBase import TSimShipBase, Pool
from SimMath import calcTrueHeading, calcDistanceHaversine, calcTargetPosition, convertionLonlatToXY
import time
import CPAModel
import database as db
import VOModel
import CollisionRiskModel


pool = Pool(maxlen = 10)

'''
Pool 数据缓冲池:
    缓冲池的大小设置后固定，例如为10，即len(pool) = 10，表示记录最近10个时刻的系统状态数据
    每个时刻的系统状态数据包括其中每条船的状态,这里表示为 [{$ship1$}, {$ship2$}, {$ship3$}, ...]
    那么这个缓冲池在运行时内部的结构如下所示：

        pool = [
            {'time': time1, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
            {'time': time2, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
            ...
            {'time': timen, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
        ]

    其中 {$ship1$} 示例:
    {
        "Time": 120,
        "type": 15,
        "mmsi": "2130001",
        "length": 22.0,
        "width": 7.0,
        "sName": "Ferry0.48704",
        "lon": 118.60009834252573,
        "lat": 31.939050966970306,
        "head": 241.63576226204026,
        "rot": 0.0,
        "speed": 3.876,
        "next": 33,
        "nextLon": 118.62957717009711,
        "nextLat": 31.947045209517565
    }
'''
class TSimShipCargo(TSimShipBase):

    def initData(self, Data = {}):
        # 下面添加此类 "额外的" 参数
        self.sName = Data.get('sName', 'shipName')
        self.color = Data.get('color', 'black')
        self.lonold = self.lon
        self.latold = self.lat
        self.DCPA = None           #DCPA值
        self.TCPA = None           #TCPA值

    def saveData(self):
        return {
                "Time": self.getTime(),
                "type": self.type,
                "mmsi": self.id,
                "length": self.length,
                "width": self.width,
                "lon": self.lon,
                "lat": self.lat,
                "head": self.head,
                "rot": self.rot,
                "speed": self.speed,
                "next": self.next,
                #"memo": self.__dict__
        }

    def NavigateModel(self):
        '''
        # 此方法可能不需要修改
        # 驾驶决策：根据当前各项因素计算如果没有干扰，依据预设航线前进所需要的正常/恢复正常参数
        # 直接修改 head, speed, rot等
        # 本处采用简单决策，如果需要更精确的计算，可在子类中继承重写即可

        # 首先检查是否到达拐点范围，如果是，以下一拐点为目标，调整head，这里考虑匀速运动
        # 如果没有下一拐点（到达终点），则本船阶段仿真结束，船舶消失
        nextPoint = self.track[self.next]
        (targetLon, targetLat) = nextPoint
        if self.isReachPoint(targetLon, targetLat):
            # 到达拐点，获取下一拐点
            self.next = self.next + 1
            (targetLon, targetLat) = nextPoint

            # 检查是否到达终点，如果是，则本船阶段仿真结束
            if self.next == len(self.track):
                # 终点抵达，设置仿真结束标志
                self.stop = True

        # 以拐点为目标，调整head，这里仅仅考虑匀速运动，故而 speed 和 rot 不变
        # 由于大地圆球影响，不能继续沿用之前的head参数
        self.head = calcTrueHeading(self.lon, self.lat, targetLon, targetLat)
        '''
        # TODO
        super().NavigateModel()
        if pool.length == 1:
            self.stop = True
        pass

    def ShipMotionModel(self):
        # 不带船舶自主操纵函数 # 需要修正转换为经纬度
        self.lonold, self.latold = self.lon, self.lat
        super().ShipMotionModel()

class TSimShipFerry(TSimShipBase):
    def __init__(self, Data):
        super().__init__(Data = Data)
        self.initData(Data)

    def initData(self, Data = {}):
        # 下面添加此类 "额外的" 参数
        #jfzhang增加的参数
        self.START = [118.6161334635414, 31.949146662009515]#渡船的起点坐标
        self.DEST = [118.63048617208129, 31.946307267990136]#渡船的终点坐标

        self.RUDDER_MAX = 35       #舵角的最大值，取35°，不需要保存到数据库
        self.K         = 0.0579    #旋回性指数，每条船舶是固定的，不需要保存到数据库
        self.T         = 69.9784   #追随性指数，每条船舶是固定的，不需要保存到数据库
        self.delta     = Data.get('delta', 0.0) #船舶在当前时刻的舵角，正值为右转，负值为左转
        self.rot_old  = 0.0       #船舶上一时刻的角速度，单位是°/s，如何更新需要确定
        self.rot      = Data.get('rot', 0.0)  #船舶当前时刻的角速度，单位是°/s
        self.sName = Data.get('sName', 'shipName')
        self.color = Data.get('color', 'black')
        #记录船舶与其他船舶之间碰撞风险最大的那条船数据
        self.risk_ship = None      #碰撞风险最大船舶的MMSI
        self.collision_risk = None #碰撞风险值
        self.DCPA = None           #DCPA值
        self.TCPA = None           #TCPA值
        self.lonold = self.lon
        self.latold = self.lat
        self.VOargs = {}
        self.AntiCollisionState = False

    # 重写saveData 以定制返回
    def saveData(self):
        # 蔡明佑函数调用
        # 为了给函数中self.参数赋值，对每个函数进行调用
        # 调用计算当前渡船碰撞风险和最大风险目标船的函数
        self.CollisionRisk_max()
        return {
                "Time": self.getTime(),
                "type": self.type,
                "mmsi": self.id,
                "length": self.length,
                "width": self.width,
                "sName": self.sName,
                "lon": self.lon,
                "lat": self.lat,
                "head": self.head,
                "rot": self.rot,
                "speed": self.speed,
                "next": self.next,
                "DCPA": self.DCPA,
                "TCPA": self.TCPA,
                "risk_ship": self.risk_ship, #碰撞风险最大船舶的MMSI
                "collision_risk": self.collision_risk, #碰撞风险值
                "VOargs": self.VOargs, 
        }

    #避碰决策模块
    def TurnRight(self,):#向右转向，舵角每次转2°
        # print('TurnRight')
        self.AntiCollisionState = True
        if self.delta <= self.RUDDER_MAX - 2:
            self.delta += 2

    def TurnLeft(self,):#向左转向，舵角每次转2°
        self.AntiCollisionState = True
        # print('TurnLeft')
        if self.delta >= -self.RUDDER_MAX + 2:
            self.delta -= 2

    def Turnhead(self,):#舵角不变
        #print("舵角不变，角速度：", self.rot)
        self.AntiCollisionState = True
        self.delta -= 0

    #计算渡船按照舵角增加turning_angle（正值为向右操舵，负值为向左操舵）航行time时间后的位置和航向
    def predict_forward(self, turning_angle, time):
        if turning_angle==2:
            if self.delta <= self.RUDDER_MAX - 2:
                delta_t = self.delta + turning_angle
            else:
                delta_t = self.delta + 0
        elif turning_angle==-2:
            if self.delta >= -self.RUDDER_MAX + 2:
                delta_t = self.delta + turning_angle
            else:
                delta_t = self.delta + 0
        else:        
            delta_t = self.delta + 0
        #print('delta_t',delta_t)
        lon_t = self.lon
        lat_t = self.lat
        head_t = self.head

        # gama_old = self.rot_old
        gama_old = self.rot
        for _ in range(time):
            gama = gama_old + (self.K * delta_t - gama_old) / self.T
            gama_old = gama
            # gama = gama_t

            head_t += gama_old
            head_t %= 360.0
            distance = self.speed
            lon_t, lat_t = calcTargetPosition(lon_t, lat_t, head_t, distance)
        head_t %= 360.0
        return lon_t, lat_t, head_t

    #计算渡船向前预测一段时间后与一条目标船之间的奖励值
    def CaculateRewards(self,lon_s, lat_s, head_s, speed_s, lon_t, lat_t, head_t, speed_t):
        dcpa, tcpa = CPAModel.calcCPA(lon_s, lat_s, head_s, speed_s, lon_t, lat_t, head_t, speed_t)
        print('dcpa tcpa',dcpa,tcpa)
        distance_now=calcDistanceHaversine(lon_s, lat_s, lon_t, lat_t)
        SDA = 100 #设置的安全距离，后期需要调整
        if (abs(dcpa) < SDA):#& ( tcpa > -100 ):#两船之间的DCPA小于安全距离
            r_safe = abs(dcpa) / SDA
            r_course = 0
            r_devdest = 0
        #elif (abs(distance_now) < SDA) &( tcpa <= (-100) ):#两船之间的DCPA小于安全距离
            #r_safe = distance_now/ SDA
            #r_course = 0
            #r_devdest = 0   
        
        else:#两船之间的DCPA大于安全距离
            #船舶当前位置指向终点的航向
            course_dest = calcTrueHeading(lon_s, lat_s, self.DEST[0], self.DEST[1])
            course_diff = abs(course_dest - head_s)#船舶当前的航向和朝向目标航向之间的差值
            if course_diff >180:
                course_diff=360-course_diff
            ######################################################################################
            #这段代码是计算渡船与它起始点连线之间的距离                     
            course_start_dest = calcTrueHeading(self.START[0], self.START[1], self.DEST[0], self.DEST[1])
            distance_start_dest=calcDistanceHaversine(lon_s, lat_s, self.DEST[0], self.DEST[1])
            
            #vec_s_t = [self.DEST[0]-self.START[0], self.DEST[1]-self.START[1]]#渡船起点到终点的方向向量
            #k = (vec_s_t[1]*self.lon - vec_s_t[0]*self.lat)/(vec_s_t[0]**2 + vec_s_t[1]**2)
            #k = -(lon_s * vec_s_t[0] + lat_s * vec_s_t[1])/(vec_f[0] * vec_s_t[0] + vec_f[1] * vec_s_t[1])
            #point_temp = [lon_s + k * vec_f[0], lat_s + k * vec_f[1]] #计算本船与起点终点航线垂直交点坐标
            #point_temp = [lon_s - k * vec_s_t[1], lat_s + k * vec_s_t[0]]
            #devdest = calcDistanceHaversine(lon_s, lat_s, point_temp[0], point_temp[1])
            devdest=abs(distance_start_dest*math.sin(math.radians(course_start_dest-course_dest)))
            ######################################################################################
            #if abs(dcpa) > SDA:
            r_safe = 1
            #else:
              #  r_safe = distance_now/ SDA
            if  calcDistanceHaversine(self.lon, self.lat, self.DEST[0], self.DEST[1])<150:
                k1=0
                k2=1
            else:
                k1=0.5
                k2=0.5
                
            r_course = k1 * math.exp(-course_diff/30)
            #print('course_diff',course_diff)
            r_devdest = k2 * math.exp(-distance_start_dest/(500))
            #print('devdest',devdest)
            print('r_safe + r_course + r_devdest',r_safe + r_course + r_devdest)
            #input()
        return r_safe + r_course + r_devdest

    # 决定船舶舵角应该向左转向、保持不变，或者向右转向
    def AntiCollision(self):
        # 获取当前时刻所有船舶数据
        data = pool.getLast()
        self.AntiCollisionState = False
        # 将渡船数据和其他船舶数据分别存进两个列表中
        #print('data:', data)
        ferry_data = []
        TS_data = []
        if data == []:
            return
        data = data[0]
        # 所有渡船mmsi列表
        #all_ferry_mmsi = [213001, 213002]
        #all_ferry_mmsi = [413827443, 413798041, 413827446, 413827447, 413812634, 413795787]
        for ship in data:
            #if ship['mmsi'] in all_ferry_mmsi:
            if ship == 'Time':
                continue
            if ship != str(self.id):
                # TS_data.append(data[ship])
                TS_data.append(data.get(ship))
        if len(TS_data):
            for target_ship in TS_data:#计算与所有目标船之间的奖励值，根据渡船和目标船的距离加权
                lon_t = target_ship['lon']
                lat_t = target_ship['lat']
                # 计算两船之间的相对距离
                Dr = calcDistanceHaversine(self.lon, self.lat, lon_t, lat_t)
            #print(min(Dr))
        else:
            Dr = 800            
        if math.floor(Dr) < 800:
            time_forward = 30 
        elif calcDistanceHaversine(self.lon, self.lat, self.DEST[0], self.DEST[1])<200:
            time_forward = 30#
        else:
            time_forward = 100 #船舶向前预测100秒
        # time_forward=100
        reward_left = []#假如向左操舵，与所有目标船之间的奖励值
        reward_head = []#假如保持舵角，与所有目标船之间的奖励值
        reward_right = []#假如向右操舵，与所有目标船之间的奖励值
        dist = []        #渡船与所有目标船之间的距离

        # if not TS_data:
        if len(TS_data) == 0:#如果目标船列表为空，直接返回
            return

        for target_ship in TS_data:#计算与所有目标船之间的奖励值，根据渡船和目标船的距离加权
            lon_t = target_ship['lon']
            lat_t = target_ship['lat']
            head_t = target_ship['head']
            speed_t = target_ship['speed']

            dist.append(calcDistanceHaversine(self.lon, self.lat, lon_t, lat_t))
            #目标船保持航向航速向前航行100秒后的位置
            lon_t, lat_t = calcTargetPosition(lon_t, lat_t, head_t, speed_t*time_forward)

            #计算假设渡船舵角向右增加2度，100秒后的位置和航向
            lon_f, lat_f, head_f = self.predict_forward(2, time_forward)
            reward_temp = self.CaculateRewards(lon_f, lat_f, head_f, self.speed,
                                               lon_t, lat_t, head_t, speed_t)
            reward_right.append(reward_temp)

            #计算假设渡船舵角保持不变，100秒后的位置和航向
            lon_f, lat_f, head_f = self.predict_forward(0, time_forward)
            reward_temp = self.CaculateRewards(lon_f, lat_f, head_f, self.speed,
                                               lon_t, lat_t, head_t, speed_t)
            reward_head.append(reward_temp)

            #计算假设渡船舵角向左操舵2度，100秒后的位置和航向
            lon_f, lat_f, head_f = self.predict_forward(-2, time_forward)
            reward_temp = self.CaculateRewards(lon_f, lat_f, head_f, self.speed,
                                               lon_t, lat_t, head_t, speed_t)
            reward_left.append(reward_temp)
            '''
            暂时将计算VO图的调用放到这里
            对于多艘目标船舶 需要分别计算他们的VO参数并按区域取集合操作
            对于一艘目标船舶 直接计算即可
            '''
            self.VOargs, _ = VOModel.calcVelocity_Obstacle_Model(self.saveData(), target_ship)

        sum_dist = sum(dist)
        reward_left_total = 0#假如向左操舵，与所有目标船之间的奖励值的加权平均值
        reward_head_total = 0#假如保持舵角，与所有目标船之间的奖励值的加权平均值
        reward_right_total = 0#假如向右操舵，与所有目标船之间的奖励值的加权平均值
        #计算与所有目标船之间奖励的加权平均值
        for i in range(len(dist)):
            dist[i] /= sum_dist
            reward_left_total += dist[i]*reward_left[i]
            reward_head_total += dist[i]*reward_head[i]
            reward_right_total += dist[i]*reward_right[i]
        print('reward_left_total, reward_head_total, reward_right_total =', reward_left_total, reward_head_total, reward_right_total)
        #如果向左转向的总奖励值大，则向左转
        if (reward_left_total >= reward_head_total) & ( reward_left_total > reward_right_total):
            self.TurnLeft()
        elif (reward_right_total >= reward_head_total)  & ( reward_right_total >= reward_left_total):#如果向右转向的总奖励值大，则向右转
            self.TurnRight()
        else:
            self.Turnhead()

    def NavigateModel(self):
        '''
        # 此方法可能不需要修改
        '''
        if not self.AntiCollisionState:
            super().NavigateModel()
        # TODO此处有问题待解决
        if len(pool.getLast()) == 2:
            self.stop = True
        pass

    def ShipMotionModel(self):
        # 根据水动力参数，前进一步，修改新的lon,lat位置等参数
        # 默认：线性模型，如果需要更精确详细的计算，可参考水动力学模型后，在子类中继承重写即可
        # lon, lat: 起始坐标
        # speed: 航速，待统一转换，初步单位为 m/s
        # head: 航向角，以真北为基准顺时针度量到航向线的角度
        # distance：本周期内，船舶行走的距离长度，初步单位为米

        #更新船舶航向和角速度
        rot_temp = self.rot_old + (self.K * self.delta - self.rot_old) / self.T
        self.rot_old = self.rot
        self.rot = rot_temp
        print('rot',self.rot)
        print('delta:',self.delta)
        self.head += self.rot
        self.head = self.head % 360
        # print("ship motion head:", self.head)
        # 一个周期后的行进距离，由于仿真间隔固定时刻为 1s，所以
        distance = self.speed # 单位为米

        # 不带船舶自主操纵函数 # 需要修正转换为经纬度
        self.lonold, self.latold = self.lon, self.lat
        self.lon, self.lat = calcTargetPosition(self.lon, self.lat, self.head, distance)
        #print('渡船信息：', self.lon, self.lat, self.head, self.speed)
        #print('最近会遇距离：', self.DCPA)

    def calcRelativeSpeed(self,head_ferry,speed_ferry,head_TS,speed_TS):
        # 计算航向差
        Cr = abs(head_TS - head_ferry)
        # 计算相对航速
        Vr = (speed_ferry ** 2 + speed_TS ** 2 - 2 * speed_ferry * speed_TS *
              math.cos(Cr / 180 * math.pi)) ** 0.5
        return Vr

    #板桥汽渡水域所有渡船信息
        # {'mmsi':413827443,'name':'BANQIAOQIDU 1','length':73,'width':14}
        # {'mmsi':413798041,'name':'BANXINERHAO','length':80,'width':15}
        # {'mmsi':413827446,'name':'BANQIAOQIDU 4','length':80,'width':15}
        # {'mmsi':413827447,'name':'BANQIAOQIDU 5','length':77,'width':20}
        # {'mmsi':413812634,'name':'BANQIAOQIDU 6','length':80,'width':20}
        # {'mmsi':413795787,'name':'BANQIAOQIDU 7','length':80,'width':20}

    ###########################################################################################################
    #计算碰撞风险模块

    # 隶属度计算,隶属函数分为三类
    # a,b,c为三角形隶属度函数的三个顶点，x为参数（相对距离、相对速度、DCPA、TCPA）的实际值，y为一个评语（隶属于低、中、高碰撞风险）的隶属度
    def membership2(self, x, a, b, c):
        if x < a:
            y = 0
        elif ((x >= a) & (x <= b)):
            y = (x - a) / (b - a)
        elif ((x > b) & (x <= c)):
            y = (c - x) / (c - b)
        elif x > c:
            y = 0
        return y

    def membership1(self, x, b, c):
        if x < b:
            y = 1
        elif ((x >= b) & (x <= c)):
            y = (c - x) / (c - b)
        elif x > c:
            y = 0
        return y

    def membership3(self, x, a, b):
        if x < a:
            y = 0
        elif ((x >= a) & (x <= b)):
            y = (x - a) / (b - a)
        elif x > b:
            y = 1
        return y

    def CollisionRisk(self, lon_TS, lat_TS, head_TS, speed_TS): # 蔡明佑负责开发
        # 计算此时渡船与一艘目标船的碰撞风险
        # 当前时刻渡船的数据
        lon_ferry = self.lon
        lat_ferry = self.lat
        head_ferry = self.head
        speed_ferry = self.speed
        # 计算两船之间的相对距离
        Dr = calcDistanceHaversine(lon_ferry, lat_ferry, lon_TS, lat_TS)
        # 计算相对航速
        Vr = self.calcRelativeSpeed(head_ferry, speed_ferry, head_TS, speed_TS)
        DCPA, TCPA = CPAModel.calcCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
        # 计算DCPA
        # DCPA = self.calcDCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
        # 计算TCPA
        # TCPA = self.calcTCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
        # 根据各个指标取值计算其隶属于低、中、高碰撞风险的隶属度，列表中各值依次代表该指标隶属于低、中、高风险的隶属度
        # 求相对距离的隶属度
        membership_Dr = [self.membership3(Dr, 320, 800), self.membership2(Dr, 80, 320, 800),
                            self.membership1(Dr, 80, 320)]
        # 求相对速度的隶属度
        membership_Vr = [self.membership1(Vr, 2, 6), self.membership2(Vr, 2, 6, 8), self.membership3(Vr, 6, 8)]
        # 求DCPA的隶属度
        membership_DCPA = [self.membership3(DCPA, 200, 500), self.membership2(DCPA, 50, 200, 500),
                            self.membership1(DCPA, 50, 200)]
        # 求TCPA的隶属度
        membership_TCPA = [self.membership3(TCPA, 120, 300), self.membership2(TCPA, 30, 120, 300),
                            self.membership1(TCPA, 30, 120)]
        # 根据熵权法处理历史会遇场景得到相对距离、相对速度、DCPA、TCPA的指标权重
        weight = [0.23615185, 0.0098181, 0.34989752, 0.40413254]
        # 根据各个指标的权重和隶属度合成此时碰撞风险隶属于低、中、高风险的隶属度
        # 低风险隶属度
        membership_low_risk = membership_Dr[0] * weight[0] + membership_Vr[0] * weight[1] + membership_DCPA[0] * \
                                weight[2] + membership_TCPA[0] * weight[3]
        # 中风险隶属度
        membership_medium_risk = membership_Dr[1] * weight[0] + membership_Vr[1] * weight[1] + membership_DCPA[1] * \
                                    weight[2] + membership_TCPA[1] * weight[3]
        # 高风险隶属度
        membership_high_risk = membership_Dr[2] * weight[0] + membership_Vr[2] * weight[1] + membership_DCPA[2] * \
                                weight[2] + membership_TCPA[2] * weight[3]
        membership_risk = [membership_low_risk, membership_medium_risk, membership_high_risk]
        # 归一化
        membership_risk = [i / sum(membership_risk) for i in membership_risk]
        # 设低、中、高风险的的权重分别为0,0.5,1，进行去模糊化
        risk = membership_risk[0] * 0 + membership_risk[1] * 0.5 + membership_risk[2] * 1
        return risk

    # 评估渡船横越时与其他所有船舶之间的碰撞风险，计算渡船的碰撞风险值以及与其风险最高的目标船和DCPA、TCPA
    def CollisionRisk_max(self):
        # 获取当前船舶数据
        data = pool.getLast()
        # 由于仿真开始时数据池中会产生空值，所以跳过
        if len(data) == 0:
            pass
        else:
            # 从列表中获取其中的字典
            ship_data = data[0]
            # 字典第0个键为时间，后面为各船mmsi，获取
            ship_mmsi = [i for i in ship_data][1:]
            # 本船的mmsi
            ferry_mmsi = self.id
            # 获取当前除本船外所有目标船的mmsi
            ship_mmsi.remove(str(ferry_mmsi))
            # 如果当前数据池中没有其他船，则跳过
            if ship_mmsi == []:
                pass
            else:
                # 创建列表存储本船与目标船的碰撞风险
                ferry_risk_list = []
                # 对于所有目标船的mmsi
                for TS_mmsi in ship_mmsi:
                    # 根据字典的键获取当前目标船的数据
                    TS_data = ship_data[TS_mmsi]
                    lon_TS = TS_data['lon']
                    lat_TS = TS_data['lat']
                    head_TS = TS_data['head']
                    speed_TS = TS_data['speed']
                    # 计算两船之间的相对距离
                    Dr = calcDistanceHaversine(self.lon, self.lat, lon_TS, lat_TS)
                    # 对于本条渡船，若目标船与本船距离小于500m，则计算目标船与本船的碰撞风险，否则认为此时风险为0
                    if Dr < 50000:
                        print('Dr', Dr)
                        # 用上述定义的碰撞风险函数计算两船之间的碰撞风险
                        risk = self.CollisionRisk(lon_TS, lat_TS, head_TS, speed_TS)
                        # 将渡船与该目标船的碰撞风险值加入列表中
                        ferry_risk_list.append(risk)
                    else:
                        # 当距离大于500m时，认为此时碰撞风险值为0
                        ferry_risk_list.append(0)
                # 以与其风险最大的目标船作为此时刻渡船的碰撞风险值
                ferry_risk = max(ferry_risk_list)
                self.collision_risk = ferry_risk
                # 如果碰撞风险值为0，则说明此时没有目标船在渡船半径500m以内，此时没有风险最大目标船，风险最大目标船DCPA和TCPA的值为空
                if ferry_risk == 0:
                    # 在每一时刻都进行重新赋值，防止显示之前时刻的状态
                    self.risk_ship = 'TS is out of range'
                    self.DCPA = 'TS is out of range'
                    self.TCPA = 'TS is out of range'
                else:
                    # 提取碰撞风险最大船舶的mmsi
                    risk_ship = ship_mmsi[ferry_risk_list.index(ferry_risk)]
                    self.risk_ship = risk_ship
                    # 提取碰撞风险最大船舶的数据
                    risk_ship_data = ship_data[risk_ship]
                    # 提取碰撞风险最大船舶的经度、纬度、航向、航速
                    lon_risk_ship = risk_ship_data['lon']
                    lat_risk_ship = risk_ship_data['lat']
                    head_risk_ship = risk_ship_data['head']
                    speed_risk_ship = risk_ship_data['speed']
                    # 计算渡船与碰撞风险最大船舶间的DCPA和TCPA
                    # DCPA_risk_ship = self.calcDCPA(self.lon, self.lat, self.head, self.speed, lon_risk_ship,
                    #                                lat_risk_ship, head_risk_ship, speed_risk_ship)
                    # TCPA_risk_ship = self.calcTCPA(self.lon, self.lat, self.head, self.speed, lon_risk_ship,
                    #                                lat_risk_ship, head_risk_ship, speed_risk_ship)                                      
                    # self.DCPA = DCPA_risk_ship
                    # self.TCPA = TCPA_risk_ship
                    self.DCPA, self.TCPA = CPAModel.calcCPA(self.lon, self.lat, self.head, self.speed, lon_risk_ship,
                                                   lat_risk_ship, head_risk_ship, speed_risk_ship)

    ###########################################################################################################

    #计算两条船舶的交叉角度，单位为°
    def crossingAngle(self, self_course, target_course):
        #本船速度向量
        x_1 = math.sin(self_course * math.pi /180)
        y_1 = math.cos(self_course * math.pi /180)

        #目标船速度向量
        x_2 = math.sin(target_course * math.pi /180)
        y_2 = math.cos(target_course * math.pi /180)

        theta = math.arccos(x_1 * x_2 + y_1 * y_2) * 180 / math.pi
        return theta

def testFuncOne():
    # 刘炯炯 测试函数
    Ships = []
    cData = {'mmsi': 2130003, 'length':10.0, 'width': 5.0, 'lon': 118.6249, 'lat': 31.9506, 'speed': 4.1156, #航速20节
             "sName": "Cargo.31802", "color": "red",
            'track':[
                      [118.6249, 31.9506], [118.6088,31.9410]]}#1 海里
    sShip3 = TSimShipCargo(cData)

    # 渡船
    bData = {'mmsi': 2130002, 'length':10.0, 'width': 5.0, 'lon': 118.6161334635414, 'lat': 31.949146662009515, 'speed': 4.1156, 'head': 103.12,#航速20节
             "sName": "Ferry0.48704", "color": "blue",
             'track':[
                    [118.6161334635414, 31.949146662009515], [118.63048617208129, 31.946307267990136]]}
             
    sShip2 = TSimShipFerry(bData)
    Ships.append(sShip2)
    Ships.append(sShip3)
    # 可以在这里再添加其他的船，再将其模仿上述过程append到Ships中
    return Ships

def testFuncTwo():
    # Bruce 测试函数
    Ships = []
    aData = {'mmsi': 2130001, 'length':10.0, 'width': 5.0, 'lon': 118.67364, 'lat': 32.00683, 'speed': 12, 'head': 60, #航速20节
             "sName": "Cargo.31802", "color": "red",
             'track':[
                      [118.67364, 32.00683], [118.669185, 31.997185], [118.663579, 31.988089],
                      [118.65783, 31.980157], [118.649135, 31.971795], [118.639864, 31.96426],
                      [118.632893, 31.958133], [118.623695, 31.951147], [118.616221, 31.946552],
                      [118.606303, 31.941894], [118.596674, 31.937482], [118.580935, 31.929208],
                      [118.572671, 31.923017], [118.566059, 31.915171], [118.556142, 31.904198],
                      [118.551399, 31.898741], [118.545291, 31.893468], [118.534223, 31.883167],
                      [118.529624, 31.877464], [118.525169, 31.870718], [118.521216, 31.861702],
                      [118.518557, 31.851397], [118.51712, 31.844035], [118.515251, 31.836366],
                      [118.513526, 31.832414], [118.511155, 31.828451], [118.505693, 31.821149]]}
    sShip1 = TSimShipCargo(aData)

    # 渡船
    bData = {'mmsi': 2130002, 'length':10.0, 'width': 5.0, 'lon': 118.6161334635414, 'lat': 31.949146662009515, 'speed': 1, 'head': 60, #航速20节
             "sName": "Ferry0.48704", "color": "blue",
             'track':[
                    [118.6161334635414, 31.949146662009515], [118.63048617208129, 31.946307267990136]]}
    sShip2 = TSimShipFerry(bData)
    Ships.append(sShip1)
    Ships.append(sShip2)
    return Ships

def main():
    # Ships = testFuncOne()
    Ships = testFuncTwo()
    # 绘图相关参数
    import matplotlib.pyplot as plt
    pCenter = [118.6353725,	31.979116]
    rangeX = 13000
    rangeY = 18000
    # plt.axis([-rangeX, rangeX, -rangeY, rangeY])
    # plt.ion()

    # main_fun(Ships)将自动运行，以下内容请勿修改
    def main_fun(Ships):
        flag = True # 标识是否有船没有运行完毕
        stack  = [] # 当前的船舶运行状态列表
        # 取得redis数据库连接并
        conn = db.connect(db = 1)
        while flag:
            time.sleep(0.1)
            cache = {
                "Time": ''
            }
            status_text = ''
            for ship in Ships:
                stack.append(ship.stop)
                if not ship.stop:
                    # 每个船舶对象运行的状态
                    status = ship.Run()
                    cache['Time'] = status.get('Time')
                    cache[str(status.get('mmsi'))] = status

                    # pOld = convertionLonlatToXY(pCenter, [ship.lonold, ship.latold])
                    # pNew = convertionLonlatToXY(pCenter, [ship.lon, ship.lat])
                    # plt.plot([pOld[0], pNew[0]], [pOld[1], pNew[1]], color= ship.color)
                    # plt.plot(pNew[0], pNew[1], 'ro')
                    status_text = status_text + f'\ntime:{cache["Time"]} mmsi:{ship.id},TCPA:{ship.TCPA},DCPA:{ship.DCPA}'

            # plt.title(status_text)
            # plt.pause(0.0001)
            pool.put(cache) # 将当前时刻的所有船舶状态以列表形式加入数据池
            # 写入redis数据库 
            db.opt_redis(conn, cache)
            print(cache)
            status_stack = map(lambda x: 1 if x else 0, stack)
            count = reduce(lambda x, y: x + y, status_stack)
            if count  == len(stack):
                flag = False
            stack = []
            pass
    main_fun(Ships)
    # plt.ioff()
    # plt.show()

if __name__ == '__main__':
    main()
